#include <mod/include/CMD.h>
#include <mod/include/LCD.h>

/** @fn		int CMD_Process(void)
    @brief	
    @param
    */
/*

  LABELS
  ========================================================
  START		start signal
  
  MODE=0		PID instellingen wegschrijven
  MODE=1		PID instellingen ophalen
  MODE=2		temperatuur stream uit of aanzetten
  
  LEN			len of chb
  
  P			P value
  I			I value
  D			D value
  G			gewenst
  
  VALUES
  ========================================================
  START:	0x55
  ACK:	0xAA
  NACK:	0x55
  
  PROTOCOL
  ========================================================		
  
  MARKER		DIR		|	byte	|	byte	|	byte	|
  +-----------+-----------+-----------+
  init:		RX		|	START	|			|			|
  TX		|	ACK		|			|			|
  
  data:		RX		|	START	|	MODE	|	LEN		|
  TX		|	ACK		|			|			|
  
  params:		RX		|P|I|D|G|...
  params:		TX		|P|I|D|G|...
  
  EXAMPLES:
  
  set PID parameters
  RX:		| START	|
  TX:		| ACK	|
  RX:		| START	| 0		| 4		|
  TX:		| ACK	|
  RX:		| P		| I		| D		| G		|
  
  request PID parameters
  RX:		| START	|
  TX:		| ACK	|
  RX:		| START	| 1		| 4		|
  TX:		| ACK	|
  RX:		| P		| I		| D		| G		|
  RX:		| P		| I		| D		| G		|

	*/
/*int CMD_Process(void){

	unsigned char ch=0x00,cha[3]={0,0,0},chb[50],loop;
	
	//	cha[0]		START
	//	cha[1]		MODE
	//	cha[2]		LEN
		
        //      chb[]		|P|I|D|G|
	

	//! init:RX
	while(ch!=STARTSIG){						//!< loop until start sig detected
		while(!UART0_Data_Ready());			//!< wait until UART data is ready
		ch=UART0_Read();					//!< read UART input
	}

	//! init:TX
	UART0_Write(ACKSIG);						//!< send ACK after start sig
	LCD_Write(ch,0);                             

	//! data:RX
	for(loop=0;loop<3;loop++){				//!< read 3 chars from UART
		while(!UART0_Data_Ready());			//!< wait until UART data is ready
		cha[loop]=UART0_Read();				//!< build cha[]
	}

	//! data:TX
	if((cha[2]>50)||(cha[0]!=STARTSIG)){		//!< if invalid input (LEN > 50 || START != 0x55)
		UART0_Write(NACKSIG);                  //!< send NACK sig
		LCD_Write(ch,1);
		return(-1);							//!< terminate function with return value -1	
	}else{
		UART0_Write(ACKSIG);					//!< send ACK
		LCD_Write(ch,1);
	}
	
	//! params:RX
	for(loop=0;loop<cha[2];loop++){			//!< read cha[2] (LEN) chars from UART
		while(!UART0_Data_Ready());			//!< wait until UART data is ready
		chb[loop]=UART0_Read();				//!< build chb[]
	}
	
	LCD_Write('2',2);
	
	switch(cha[1]){							//!< handle cha[1] (MODE)
		case 0 :{                           //!< read PID values
			P      =chb[0];
			I      =chb[1];
			D      =chb[2];
			gewenst=chb[3];
			break;
		}
		//! params: TX
		case 1 :{                           //!< write PID values
			UART0_Write(P);
			UART0_Write(I);
			UART0_Write(D);
			UART0_Write(gewenst);
			break;
		}
		case 2 :{							//Temperatuur stream uit of aanzetten.
			STREAM=chb[0];
			LCD_Clear();
			if(STREAM){
				LCD_Write('1',3);
				PIT_Start();
			}else{
				LCD_Write('2',3);
				PIT_Stop();
			}
			break;
		}
		default:{
			UART0_Write(ACKSIG);				//!< send ACK
			break;
		}
	}
}*/

int CMD_Process(void){
   unsigned char ch=0x00,cha[3]={0,0,0},chb[50],loop;
   while(ch!=0x55){                           //check of 0x55 is binnen gekomen als start signaal
      while(!UART0_Data_Ready());
      ch=UART0_Read();
   }
   UART0_Write(0xAA);
   LCD_Write(ch,0);                             //Stuur 0xAA als ACK
   for(loop=0;loop<3;loop++){
      while(!UART0_Data_Ready());
      cha[loop]=UART0_Read();
   }
   if((cha[2]>50)||(cha[0]!=0x55)){
      UART0_Write(0x55);                       //Stuur 0x55 als NACK
      LCD_Write(ch,1);
      return(-1);
   }else{
      UART0_Write(0xAA);                       //Stuur 0xAA als ACK
      LCD_Write(ch,1);
   }
   for(loop=0;loop<cha[2];loop++){
      while(!UART0_Data_Ready());
      chb[loop]=UART0_Read();
   }
   LCD_Write('2',2);
   switch(cha[1]){
      case 0 :{                              //PID instellingen wegschrijven
         P      =chb[0];
         I      =chb[1];
         D      =chb[2];
         gewenst=chb[3];
      }break;
      case 1 :{                              //PID instellingen ophalen
         UART0_Write(P);                       //Stuur 0xAA als ACK
         UART0_Write(I);                       //Stuur 0xAA als ACK
         UART0_Write(D);                       //Stuur 0xAA als ACK
         UART0_Write(gewenst);                       //Stuur 0xAA als ACK
      }break;
      case 2 :{                              //Temperatuur stream uit of aanzetten.
         STREAM=chb[0];
         LCD_Clear();
         if(STREAM){
            LCD_Write('1',3);
            PIT_Start();
         }else{
            LCD_Write('2',3);
            PIT_Stop();
         }
      }break;
      default:{
         UART0_Write(0x55);                       //Stuur 0xAA als ACK
      }break;
   }
}
